import rclpy
from rclpy.node import Node
from rclpy.clock import Clock
from motors_node.motor_modbus import SerialCommunication,SingleMotor
from motor_control_interfaces.msg import MotorCmd,MotorMsg
import time

class MotorController(Node):

    def __init__(self):

        super().__init__('motor_node')
        
        # 动态参数设置
        self.declare_parameter('spin_motor_id',0)
        
        # ros话题设置
        self.timer10ms = self.create_timer(0.5, self.timer500ms_callback)
        self.motor_msg_pub = self.create_publisher(MotorMsg, 'motor_msg', 50)
        self.motor_cmd_sub = self.create_subscription(MotorCmd, 'motor_cmd',self.motor_callback, 50)
        self.motor_cmd_sub
        
        # 电机参数设置：
        ## 电机数量
        self.motors_num=8
        ## 电机类型设置
        motors_type=[1,0,1,0,1,0,1,0]
        
        # 电机类初始化
        self.motors=[]
        self.motor_serial=SerialCommunication(port='/dev/ttyUSB0')
        for i in range(self.motors_num):
            motor=SingleMotor(motor_id=i+1,serial_communication=self.motor_serial,motor_type=motors_type[i])
            self.motors.append(motor)

    # 监视电机的位置和速度，由参数spin_motor_id控制
    def timer500ms_callback(self):
        motor_msg = MotorMsg()
        spin_motor_id=self.get_parameter('spin_motor_id').get_parameter_value().integer_value
        if spin_motor_id>0:
            motor_msg.total_angle = self.motors[spin_motor_id-1].motor_position_get()
            motor_msg.velocity = self.motors[spin_motor_id-1].motor_speed_get()
            self.motor_msg_pub.publish(motor_msg)

    # 根据接受的话题控制电机，每收到一次话题，就执行一次
    def motor_callback(self,msg):
        # 抛弃过时的数据
        time_msg = msg.header.stamp.sec + msg.header.stamp.nanosec * 1e-9
        time_now=self.get_clock().now().to_msg().sec + self.get_clock().now().to_msg().nanosec* 1e-9
        time_difference = time_now - time_msg
        if time_difference>0.1:
            return
        
        for i in range(self.motors_num):
            self.motors[i].montor_enable(int(msg.enable[i]))
            # 速度控制
            self.motors[i].motor_speed_set(int(msg.setvelocity[i]))
            # 位置控制
            self.motors[i].motor_position_set(int(msg.total_angle[i]))

    # disable所有电机
    def motor_disable(self):
        self.motors[0].motor_position_set(0)
        self.motors[2].motor_position_set(0)
        self.motors[4].motor_position_set(0)
        self.motors[6].motor_position_set(0)
        time.sleep(2)
        for i in range(self.motors_num):
            self.motors[i].montor_enable(0)
        
        
def main(args=None):
    rclpy.init(args=args)
    motor_node = MotorController()
    try:
        rclpy.spin(motor_node)
    finally:
        motor_node.motor_disable()
    motor_node.destroy_node()
    rclpy.shutdown()

if __name__ == '__main__':
    main()
